#include "bevdet_tracker.h"

#include "association/associate_factor.h"
#include "association/associator.h"
#include "tracker/tracker_manager.h"

using namespace bevdet::tracker;

BevdetTracker::BevdetTracker()
{
    associatorSptr_ = std::make_shared<Associator>(std::vector<std::shared_ptr<AssociateFactor>> {
        std::make_shared<PositionXAssociateFactor>("x", 60, 5),
        std::make_shared<PositionYAssociateFactor>("y", 60, 5),
        std::make_shared<HeadingAssociateFactor>("h", 100, 90),
    });

    trackerManagerSptr_ = std::make_shared<TrackerManager>();
}

ObjectSptrVector BevdetTracker::update(int64_t microTs, ObjectSptrVector objects)
{
    auto LastFusedObjects = trackerManagerSptr_->getFusedObjects();

    auto multiAssociatedObjectSptr = associatorSptr_->multiAssociate(microTs, LastFusedObjects, objects);

    trackerManagerSptr_->fuse(microTs, multiAssociatedObjectSptr);

    return trackerManagerSptr_->getFusedObjects();
}
